/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/footprint.h>
#include <cstdio>
#include <string>
#include <algorithm>
#include <vector>

using std::vector;

namespace costmap_2d
{

LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) :
        costmap_(), global_frame_(global_frame), rolling_window_(rolling_window), current_(false),
        minx_(0.0), miny_(0.0), maxx_(0.0), maxy_(0.0),
        bx0_(0), bxn_(0), by0_(0), byn_(0),
        initialized_(false), size_locked_(false),
        circumscribed_radius_(1.0), inscribed_radius_(0.1)
{
    /* track_unkown_space参数先给其内部的总地图Costmap2D设置地图
     * 数据缺省值NO_INFORMATION或者FREE_SPACE
     * track_unknown_space is true
     * track_unknown: 如果为false，则每个像素具有两种状态之一：致命障碍或自由。如果为true，则每个像素具有三种状态之一：致命障碍、自由或未知。
     */
#ifdef SEMANTIC
    if (track_unknown)
    {
        costmap_.setDefaultValue(NO_INFORMATION);  // old rules
        semantic_costmap_.setDefaultValue(toOri(XJU_COST_NO_INFORMATION, XJU_OPTION_INIT)); // new rules
    }
    else
    {
        costmap_.setDefaultValue(FREE_SPACE);
        semantic_costmap_.setDefaultValue(toOri(XJU_COST_NO_INFORMATION, XJU_OPTION_INIT));
    }
#else
    if (track_unknown) costmap_.setDefaultValue(NO_INFORMATION);
    else costmap_.setDefaultValue(FREE_SPACE);
#endif
}

LayeredCostmap::~LayeredCostmap()
{
    while (!plugins_.empty()) plugins_.pop_back();
}
// 调整地图大小
void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double resolution,
                               double origin_x, double origin_y, bool size_locked)
{
    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
    size_locked_ = size_locked;
    // 调用costmap_的resizeMap方法
    ROS_WARN_STREAM("size_x: " << size_x << " size_y: " << size_y << " resolution: " << resolution);
    costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
#ifdef SEMANTIC
//    boost::unique_lock<Costmap2D::mutex_t> semantic_lock(*(semantic_costmap_.getMutex()));
    semantic_costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
#endif
    // 然后根据plugin对每一层的地图调用其父类Costmap2D成员的initial方法，将plugin所指向的每一层地图的大小都设置为costmap_数据成员一样的空间大小
    for (auto & plugin : plugins_) plugin->matchSize();
}

// 更新地图: 更新边界、更新代价值
void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
{
#ifdef SEMANTIC
    boost::unique_lock<Costmap2D::mutex_t> semantic_lock(*(semantic_costmap_.getMutex()));
    double new_origin_x, new_origin_y;
    if (rolling_window_)
    {
        new_origin_x = robot_x - semantic_costmap_.getSizeInMetersX() / 2;
        new_origin_y = robot_y - semantic_costmap_.getSizeInMetersY() / 2;
        semantic_costmap_.updateOrigin(new_origin_x, new_origin_y);
    }
    if (plugins_.empty()) return;
    minx_ = miny_ = 1e30;
    maxx_ = maxy_ = -1e30;
    // 依次更新每个layer的区域，这样在每个运行周期内减少了数据拷贝的操作时间。
    for (auto & plugin : plugins_)
    {
        if (!plugin->isEnabled()) continue;
        double prev_minx = minx_;
        double prev_miny = miny_;
        double prev_maxx = maxx_;
        double prev_maxy = maxy_;
        // updateBounds传入的是一个矩形范围
        plugin->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
        if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
        {
            ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
                                   "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
                              prev_minx, prev_miny, prev_maxx, prev_maxy,
                              minx_, miny_, maxx_, maxy_,
                              plugin->getName().c_str());
        }
    }
    int x0, xn, y0, yn;
    // 从世界坐标（wx, wy）向地图坐标(mx, my)转换,map中强制加入边界
    semantic_costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
    semantic_costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);
    // 范围更新
    ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
    if (xn < x0 || yn < y0) return;
    semantic_costmap_.resetMap(x0, y0, xn, yn);
    for (auto & plugin : plugins_)
    {
        if (plugin->isEnabled()) plugin->updateCosts(semantic_costmap_, x0, y0, xn, yn);
    }
    if (rolling_window_) costmap_.updateOrigin(new_origin_x, new_origin_y);
    for (auto y = y0; y < yn; ++y)
    {
        for (auto x = x0; x < xn; ++x)
        {
            costmap_.setCost(x, y, interpretValue(semantic_costmap_.getCost(x, y)));
        }
    }
#else
    // Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
    // implement thread unsafe updateBounds() functions.
    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
    // 如果用滚动的代价地图，需要用机器人的位置更新原点
    // if we're using a rolling buffer costmap... we need to update the origin using the robot's position
    if (rolling_window_)
    {
        double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
        double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
        costmap_.updateOrigin(new_origin_x, new_origin_y);
    }
    if (plugins_.empty()) return;
    minx_ = miny_ = 1e30;
    maxx_ = maxy_ = -1e30;
    // 依次更新每个layer的区域，这样在每个运行周期内减少了数据拷贝的操作时间。
    for (auto & plugin : plugins_)
    {
        if (!plugin->isEnabled()) continue;
        double prev_minx = minx_;
        double prev_miny = miny_;
        double prev_maxx = maxx_;
        double prev_maxy = maxy_;
        // updateBounds传入的是一个矩形范围
        plugin->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
        if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
        {
            ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
                                   "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
                              prev_minx, prev_miny, prev_maxx, prev_maxy,
                              minx_, miny_, maxx_, maxy_,
                              plugin->getName().c_str());
        }
    }
    int x0, xn, y0, yn;
    // 从世界坐标（wx, wy）向地图坐标(mx, my)转换,map中强制加入边界
    costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
    costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);
    x0 = std::max(0, x0);
    xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
    y0 = std::max(0, y0);
    yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
    // 范围更新
    ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
    if (xn < x0 || yn < y0) return;
    costmap_.resetMap(x0, y0, xn, yn);
    for (auto & plugin : plugins_)
    {
        if (plugin->isEnabled()) plugin->updateCosts(costmap_, x0, y0, xn, yn);
    }
#endif
    bx0_ = x0;
    bxn_ = xn;
    by0_ = y0;
    byn_ = yn;
    initialized_ = true;
}
// 显示当前状态
bool LayeredCostmap::isCurrent()
{
    current_ = true;
    for (auto & plugin : plugins_)
    {
        // 是否启用
        if (plugin->isEnabled()) current_ = current_ && plugin->isCurrent();
    }
    return current_;
}
// 设置机器人足迹footprint
void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::Point> &footprint_spec)
{
    footprint_ = footprint_spec;
    costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
    for (auto & plugin : plugins_) plugin->onFootprintChanged();
}

#ifdef SEMANTIC
unsigned char LayeredCostmap::interpretValue(unsigned char old_grid)
{
    switch (toXJUcost(old_grid))
    {
        case XJU_COST_NO_INFORMATION: return NO_INFORMATION;
        case XJU_COST_LETHAL_OBSTACLE: return LETHAL_OBSTACLE;
        case XJU_COST_INSCRIBED_INFLATED_OBSTACLE: return INSCRIBED_INFLATED_OBSTACLE;
        default: return old_grid * 8;
    }
}
#endif
}  // namespace costmap_2d
